#include <sys/signal.h>

#include "main.h"
#include "adc.h"
#include "dma.h"
#include "entry_TraceCar.h"
#include "i2c.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"
#include "pid.h"
#include "RDdata.h"

#define BlackToWhite 1350//1400 5401教室后面场地//-1800
#define WhiteToBlack 700//700 5401教室后面场地
#define High_Speed 2500
#define Basic_Speed 1500
#define Low_Speed 500
//声光提醒第18行，电机第33行,题目开始：第64行

/*BianLiang ShengMing Begin*/
//传感器，pid结构体声明
JY901_Angle IMU;
tPID PID_Straight;
tPID PID_Trace;
tPID PID_HighSpeed_Trace;
tPID PID_BasicSpeed_Trace;
tPID PID_LowSpeed_Trace;
//题目flag
uint8_t Trace_Black_Flag=0;
uint8_t Forward_Flag=0;
uint8_t Around_Trace_Flag=0;
uint8_t Cross_Half_Flag=0;
uint8_t Around_Trace_Two_Flag=0;
uint8_t Cross_Full_Flag=0;
Trace_Excursion Trace_Ex;

float Final;
float Final_Basic;
float Final_Low;

/*BianLiang ShengMing End*/

/**************************************************************************
函数功能：蜂鸣器响，led亮
入口参数：无
返回  值：无
**************************************************************************/
void worning(uint8_t flag)
{
    if(flag) {
        HAL_GPIO_WritePin(Beef_GPIO_Port, Beef_Pin, GPIO_PIN_RESET);
        HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_SET);
    } else{
        HAL_GPIO_WritePin(Beef_GPIO_Port, Beef_Pin, GPIO_PIN_SET);
        HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_RESET);
    }
}

/**************************************************************************
函数功能：控制正反转,设置速度
入口参数：左、右轮速度
返回  值：无
**************************************************************************/
void Load(int moto1,int moto2)
{
    //数据正负号，对应正反转
    if(moto1>0){
        HAL_GPIO_WritePin(BIN1_GPIO_Port,BIN1_Pin,GPIO_PIN_SET);
        HAL_GPIO_WritePin(BIN2_GPIO_Port,BIN2_Pin,GPIO_PIN_RESET);//正转
        __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_2,moto1);
    }
    else{
        HAL_GPIO_WritePin(BIN1_GPIO_Port,BIN1_Pin,GPIO_PIN_RESET);
        HAL_GPIO_WritePin(BIN2_GPIO_Port,BIN2_Pin,GPIO_PIN_SET);//反转
        __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_2,-moto1);
    }

    if(moto2>0){
        HAL_GPIO_WritePin(AIN1_GPIO_Port,AIN1_Pin,GPIO_PIN_SET);
        HAL_GPIO_WritePin(AIN2_GPIO_Port,AIN2_Pin,GPIO_PIN_RESET);//正转
        __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1,moto2);
    }
    else{
        HAL_GPIO_WritePin(AIN1_GPIO_Port,AIN1_Pin,GPIO_PIN_RESET);
        HAL_GPIO_WritePin(AIN2_GPIO_Port,AIN2_Pin,GPIO_PIN_SET);//反转
        __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1	,-moto2);
    }
}

/**************************************************************************
函数功能：初始化题目标志位为0
入口参数：无
返回  值：无
**************************************************************************/
void Question_Init(void) {
    Trace_Black_Flag=0;
    Forward_Flag=0;
    Around_Trace_Flag=0;
    Cross_Half_Flag=0;
    Around_Trace_Two_Flag=0;
    Cross_Full_Flag=0;
}

/**************************************************************************
                                第一题
函数功能：将小车放置在A点，小车自动沿引导线逆时针行驶到达D点后停车，停车后发出声光提示
入口参数：无
返回  值：无
**************************************************************************/
void Trace_Black() {
    static float Trace_Bias;
    static uint16_t time_slice=0;
    //循迹
    Trace_RDDat(&Trace_Ex);
    JY901_RDDat(&IMU);
    time_slice++;
    Final = PID_Trace_Calc(&PID_HighSpeed_Trace,Trace_Ex.Trace_Bias,IMU.w[2]);
    Final_Basic = PID_Trace_Calc(&PID_BasicSpeed_Trace,Trace_Ex.Trace_Bias,IMU.w[2]);
    //起步时中速，提高巡线的准确性，之后提速
    if(time_slice<=650) {
        Load(Basic_Speed+Final_Basic,Basic_Speed-Final_Basic);
    }else if(time_slice>650&&time_slice<1050) {
        Load(High_Speed+Final,High_Speed-Final);
    }
    else if(time_slice>=1050){
        Load(Basic_Speed+Final_Basic,Basic_Speed-Final_Basic);
        time_slice--;//防止时间记录超过最大值，变为负数
        //白色停止
        if(Trace_Ex.Trace_Bias>BlackToWhite||Trace_Ex.Trace_Bias<-BlackToWhite-250) {
            time_slice++;
            Load(0,0);
            worning(1);
            //声光提醒一秒
            if(time_slice>=1250) {
                worning(0);
                Trace_Black_Flag=0;
            }

        }
    }
}

/**************************************************************************
                                第二题
函数功能：将小车放置在A点，顺时针行驶到达D点后停车，停车后发出声光提示。
入口参数：无
返回  值：无
**************************************************************************/
void Forward_Trace() {
    static float Trace_Bias;
    static uint8_t time_slice=0;
    static uint8_t time_slice_flag=0;
    //读取angle初值
    if(Forward_Flag==1) {
        Trace_Bias=IMU.Angle[2];
        Forward_Flag=2;
    }
    //直线行走
    JY901_RDDat(&IMU);
    Final=PID_Trace_Calc(&PID_Straight,Trace_Bias-IMU.Angle[2],IMU.w[2]);
    Load(High_Speed+(int)Final,High_Speed-(int)Final);
    Trace_RDDat(&Trace_Ex);
    //黑线检测
    if(Trace_Ex.Trace_Bias<WhiteToBlack&&Trace_Ex.Trace_Bias>-WhiteToBlack) {
        time_slice_flag=1;
    }
    //延时，为了让小车车身停在线上
    if(time_slice_flag==1) {
        time_slice++;
    }
    if(time_slice>=20) {
        worning(1);
        Load(0,0);
        //声光提醒一秒
        if(time_slice>=220) {
            worning(0);
            Forward_Flag=0;//退出函数的标志
        }
    }//小车到达终点，停止
}

/**************************************************************************
                                第三题
函数功能：将小车放置在A点，逆时针方向沿引导线行驶一圈，再次到达A点时自动停车，停车后发出声光提示。
入口参数：无
返回  值：无
**************************************************************************/
void Around_Trace() {
    static float Trace_Bias;
    static uint16_t time_slice=0;
    static uint8_t time_slice_flag=0;
    //读取角度初值
    JY901_RDDat(&IMU);
    Trace_RDDat(&Trace_Ex);
    if(Around_Trace_Flag==1) {
        Trace_Bias=IMU.Angle[2];
        Around_Trace_Flag=2;
    }
    //循迹
    else if(Around_Trace_Flag==2) {
        time_slice++;

        //起步时慢速，提高巡线的准确性，之后提速
        if(time_slice<=650) {
            Final_Basic=PID_Trace_Calc(&PID_BasicSpeed_Trace,Trace_Ex.Trace_Bias,IMU.w[2]);
            Load(Basic_Speed+Final_Basic,Basic_Speed-Final_Basic);
        }else if(time_slice>650&&time_slice<1050) {
            Final = PID_Trace_Calc(&PID_Trace,Trace_Ex.Trace_Bias,IMU.w[2]);
            Load(High_Speed+(int)Final,High_Speed-(int)Final);
        }
        else if(time_slice>=1050) {
            Final_Basic=PID_Trace_Calc(&PID_BasicSpeed_Trace,Trace_Ex.Trace_Bias,IMU.w[2]);
            Load(Basic_Speed+Final_Basic,Basic_Speed-Final_Basic);
            time_slice--;//防止时间记录超过最大值，变为负数
            //检测到白色，题目标志位进入直线阶段
            if(Trace_Ex.Trace_Bias>BlackToWhite||Trace_Ex.Trace_Bias<-BlackToWhite-250) {
                Around_Trace_Flag=3;
                time_slice=0;
            }
        }
    }
    //直行
    else if(Around_Trace_Flag==3) {
        Final=PID_Trace_Calc(&PID_Straight,Trace_Bias-IMU.Angle[2],IMU.w[2]);
        Load(High_Speed+(int)Final,High_Speed-(int)Final);
        //判断是否检测到黑线
        if(Trace_Ex.Trace_Bias<WhiteToBlack&&Trace_Ex.Trace_Bias>-WhiteToBlack) {
            time_slice_flag=1;
        }
        //延时，为了让小车车身停在线上
        if(time_slice_flag==1) {
            time_slice++;
        }
        if(time_slice>=25) {
            worning(1);
            Load(0,0);
            //声光提醒一秒
            if(time_slice>=225) {
                worning(0);
                 Around_Trace_Flag=0;//退出函数的标志
            }
        }//小车到达终点，停止
    }
}

/**************************************************************************
                                第四题
函数功能：将小车放置在 A 点，不经过 B 直接行驶到 C(斜线)，再行驶到 D 停车，停车后发出声光提示
入口参数：无
返回  值：无
**************************************************************************/
void Cross_half() {
    static float Trace_Bias;
    static uint16_t time_slice=0;
    static uint8_t time_slice_Stop=0;
    static uint8_t time_slice_flag=0;
    static uint8_t Final_Turn=0;
    //自传角度，然后给angle初值
    JY901_RDDat(&IMU);
    Trace_RDDat(&Trace_Ex);
    if(Cross_Half_Flag==1) {
        Load(500,-500);
        time_slice++;
        if(time_slice>=140) {
            Cross_Half_Flag=2;
            Trace_Bias=IMU.Angle[2];
        }
    }
    //直行
    else if(Cross_Half_Flag==2) {
        Final=PID_Trace_Calc(&PID_Straight,Trace_Bias-IMU.Angle[2],IMU.w[2]);
        Load(High_Speed+(int)Final,High_Speed-(int)Final);
        //判断是否检测到黑线
        if(Trace_Ex.Trace_Bias<WhiteToBlack&&Trace_Ex.Trace_Bias>-WhiteToBlack) {
            Load(0,0);
            Cross_Half_Flag=3;
        }
    }
    //循迹
    else if(Cross_Half_Flag==3) {
        if(time_slice<200) {
            Final_Low=PID_Trace_Calc(&PID_LowSpeed_Trace,Trace_Ex.Trace_Bias,IMU.w[2]);
            Load(Low_Speed+Final_Low,Low_Speed-Final_Low);
            time_slice++;//延时慢速，使小车顺利进入黑线
        }
        else if(time_slice>=200) {
            Final_Basic = PID_Trace_Calc(&PID_BasicSpeed_Trace,Trace_Ex.Trace_Bias,IMU.w[2]);
            Load(Basic_Speed+Final_Basic,Basic_Speed-Final_Basic);
            if(Trace_Ex.Trace_Bias>BlackToWhite||Trace_Ex.Trace_Bias<-BlackToWhite-250) {
                worning(1);
                Load(0,0);
                time_slice++;
                //声光提醒一秒
                if(time_slice>=550) {
                    worning(0);
                    Cross_Half_Flag=0;
                }
            }
        }
    }
}

/**************************************************************************
                                第五题
函数功能：小车放置在 A 点，行驶路线为题目3，连续行驶 2 圈后停车，时间越短越好，
每次经过 A 时需要声光提示，停车后需要声光提示。
入口参数：无
返回  值：无
**************************************************************************/
void Around_Trace_Two() {
    static float Trace_Bias;
    static uint8_t Twice_Flag=0;
    static uint32_t time_slice=0;
    static float Final_Start=0;
    static uint8_t time_slice_flag=0;
    //读取angle初值
    JY901_RDDat(&IMU);
    Trace_RDDat(&Trace_Ex);
    if(Around_Trace_Two_Flag==1) {
        Trace_Bias=IMU.Angle[2];
        Around_Trace_Two_Flag=2;
    }
    //循迹
    else if(Around_Trace_Two_Flag==2) {
        time_slice++;
        Final = PID_Trace_Calc(&PID_Trace,Trace_Ex.Trace_Bias,IMU.w[2]);
        Final_Basic = PID_Trace_Calc(&PID_BasicSpeed_Trace,Trace_Ex.Trace_Bias,IMU.w[2]);
        //起步时慢速，提高巡线的准确性，之后提速
        if(time_slice<=650) {
            Load(Basic_Speed+Final_Basic,Basic_Speed-Final_Basic);
        }else if(time_slice>650&&time_slice<1050) {
            Load(High_Speed+Final,High_Speed-Final);
        }
        else if(time_slice>=1050) {
            time_slice--;
            //检测白色
            Load(Basic_Speed+Final_Basic,Basic_Speed-Final_Basic);
            if(Trace_Ex.Trace_Bias>BlackToWhite) {
                Around_Trace_Two_Flag=3;
                time_slice=0;
            }
        }
    }
    //直行
    else if(Around_Trace_Two_Flag==3) {
        Final=PID_Trace_Calc(&PID_Straight,Trace_Bias-IMU.Angle[2],IMU.w[2]);
        Load(High_Speed+(int)Final,High_Speed-(int)Final);
        //黑线检测
        if(Trace_Ex.Trace_Bias<WhiteToBlack&&Trace_Ex.Trace_Bias>-WhiteToBlack) {
            Around_Trace_Two_Flag=4;
        }
    }
    //检测到黑线，且Twice_Flag为0（第一次到A点），回到循迹阶段进行第二圈
    else if(Around_Trace_Two_Flag==4&&Twice_Flag==0) {
        Final = PID_Trace_Calc(&PID_Trace,Trace_Ex.Trace_Bias,IMU.w[2])/1.4;
        if(time_slice<100) {
            Load(Low_Speed+(int)Final,Low_Speed-(int)Final);
            worning(1);
            time_slice++;//延时，使小车顺利进入黑线,并且给出时间进行声光提醒
        }
        else if(time_slice>=100) {
            worning(0);
            Around_Trace_Two_Flag=2;
            Twice_Flag=1;
            time_slice=0;
        }
    }else if(Around_Trace_Two_Flag==4&&Twice_Flag==1) {
        Load(0,0);
        worning(1);
        time_slice++;
        //声光提醒一秒
        if(time_slice>=200) {
            worning(0);
            Around_Trace_Two_Flag=0;
        }

    }
}


/**************************************************************************
                                第六题
函数功能：小车放置在 A 点，直接行驶到 C（斜线），然后经过 CD 圆弧行驶到 D，然后直接行驶到 B（斜线），
经过 BA 圆弧行驶到 A 后停车，每经过一个点均需要声光提示，时间越短越好。
入口参数：无
返回  值：无
**************************************************************************/
void Cross_Full() {
    static float Trace_Bias;
    static uint8_t time_slice=0;
    static uint8_t time_slice_Stop=0;
    static uint16_t time_slice_Twice=0;
    static uint8_t time_slice_flag=0;
    static uint8_t Final_Turn=0;
    static uint8_t Twice_flag=0;

    JY901_RDDat(&IMU);
    Trace_RDDat(&Trace_Ex);
    //起步自传，且读取angle初值
    if(Cross_Full_Flag==1) {
        Load(500,-500);
        worning(1);
        time_slice++;
        if(time_slice>=140) {
            Cross_Full_Flag=2;
            Trace_Bias=IMU.Angle[2];
            time_slice=0;
            worning(0);
        }
    }
    //直行，检测黑线
    else if(Cross_Full_Flag==2) {
        Final=PID_Trace_Calc(&PID_Straight,Trace_Bias-IMU.Angle[2],IMU.w[2]);
        Load(High_Speed-300+(int)Final/1.1,High_Speed-300-(int)Final/1.1);
        if(Trace_Ex.Trace_Bias<WhiteToBlack&&Trace_Ex.Trace_Bias>-WhiteToBlack) {
            //声光提醒
            worning(1);
            Load(0,0);
            Cross_Full_Flag=3;
        }
    }
    // 循迹
    else if(Cross_Full_Flag==3) {
        Final_Basic = PID_Trace_Calc(&PID_BasicSpeed_Trace,Trace_Ex.Trace_Bias,IMU.w[2]);
        Final_Low=PID_Trace_Calc(&PID_LowSpeed_Trace,Trace_Ex.Trace_Bias,IMU.w[2]);
        if(time_slice<150) {
             Load(Low_Speed+Final_Low,Low_Speed-Final_Low);
             time_slice++;//延时，使小车顺利进入黑线
        }
        else if(time_slice>=150) {
            worning(0);
            Load(Basic_Speed+Final_Basic,Basic_Speed-Final_Basic);
            if(Trace_Ex.Trace_Bias>BlackToWhite||Trace_Ex.Trace_Bias<-BlackToWhite-250) {
                //声光提醒
                worning(1);
                Cross_Full_Flag=4;
                Trace_Bias=IMU.Angle[2];
                time_slice=0;
            }
        }
    }
    //自旋转，旋转到对角线,Twice_flag为0（第一次出黑线）回到直线部分
    else if(Cross_Full_Flag==4&&Twice_flag==0) {
        Load(-500,500);
        time_slice_Twice++;
        if(time_slice_Twice>=140) {
            worning(0);
            Cross_Full_Flag=2;
            Trace_Bias=IMU.Angle[2];
            Twice_flag=1;//第二次出黑线就停止
        }
    }else if(Cross_Full_Flag==4&&Twice_flag==1) {
        Load(0,0);
        time_slice++;
        worning(1);
        //声光提醒
        if(time_slice>=150) {
            worning(0);
            Cross_Full_Flag=0;
        }

    }
}

